//
// Created by geoffrey on 24-11-16.
//

#include"DrawTrajectory.h"

// 画出轨迹

void DrawTrajectory(const vector<Matrix<double, 6, 2>,aligned_allocator<Matrix<double, 6, 2>>>& State ,const vector<Matrix<double, 6, 2>,aligned_allocator<Matrix<double, 6, 2>>>& State_d) {




    // 创建一个MATLAB数据数组工厂
    matlab::data::ArrayFactory factory;
    // 创建一个包含数据的数组
    matlab::data::TypedArray<double> x = factory.createArray<double>({State.size(),1});
    matlab::data::TypedArray<double> y = factory.createArray<double>({State.size(), 1});
    matlab::data::TypedArray<double> z = factory.createArray<double>({State.size(), 1});
    matlab::data::TypedArray<double> x_d = factory.createArray<double>({State_d.size(),1});
    matlab::data::TypedArray<double> y_d = factory.createArray<double>({State_d.size(), 1});
    matlab::data::TypedArray<double> z_d = factory.createArray<double>({State_d.size(), 1});

    for (size_t i = 0; i < State.size(); ++i) {
        x[i][0] = State[i](0,0);
        y[i][0] = State[i](1,0);
        z[i][0] = State[i](2,0);
    }
    for (size_t i = 0; i < State_d.size(); ++i) {
        x_d[i][0] = State_d[i](0,0);
        y_d[i][0] = State_d[i](1,0);
        z_d[i][0] = State_d[i](2,0);
    }
    // 将变量传递给MATLAB引擎
    unique_ptr<matlab::engine::MATLABEngine> matlabPtr  = matlab::engine::startMATLAB();
    matlabPtr->setVariable(u"x", x);
    matlabPtr->setVariable(u"y", y);
    matlabPtr->setVariable(u"z", z);
    matlabPtr->setVariable(u"x_d", x_d);
    matlabPtr->setVariable(u"y_d", y_d);
    matlabPtr->setVariable(u"z_d", z_d);


    // 调用MATLAB绘图函数
    matlabPtr->eval(u"figure;");
    matlabPtr->eval(u"plot3(x, y, z,'b');");
    matlabPtr->eval(u"title('Plot from C++');");
    //matlabPtr->eval(u"ydir('reverse');");
    //matlabPtr->eval(u"zdir('reverse');");
    matlabPtr->eval(u"xlabel('X-axis');");
    matlabPtr->eval(u"ylabel('Y-axis');");
    matlabPtr->eval(u"zlabel('Z-axis');");
    matlabPtr->eval(u"hold on;");
    matlabPtr->eval(u"plot3(x_d, y_d, z_d,'r');");
    matlabPtr->eval(u"hold off;");
    matlabPtr->eval(u"while true; pause(0.1); end");

    std::cout << "Plot created successfully!" << std::endl;



}